#include "ros/ros.h"
#include "std_msgs/Float64.h" 
#include <sstream>
 
int main(int argc, char  *argv[])
{   
    setlocale(LC_ALL,"");
    ros::init(argc,argv,"control_gazebo_demo1");
    ros::NodeHandle nh;
    ros::Publisher pub_lf = nh.advertise<std_msgs::Float64>("/gazebo_demo1/lf_position_con/command",10);
    ros::Publisher pub_lb = nh.advertise<std_msgs::Float64>("/gazebo_demo1/lb_position_con/command",10);
    ros::Publisher pub_rf = nh.advertise<std_msgs::Float64>("/gazebo_demo1/rf_position_con/command",10);
    ros::Publisher pub_rb = nh.advertise<std_msgs::Float64>("/gazebo_demo1/rb_position_con/command",10);
    std_msgs::Float64 control_gazebo_demo1;
    control_gazebo_demo1.data=0.0;
    bool flag=0;//0weizeng 1weijian
    ros::Rate r(15);
    while (ros::ok())
    {

        pub_lf.publish(control_gazebo_demo1);
        pub_lb.publish(control_gazebo_demo1);
        pub_rf.publish(control_gazebo_demo1);
        pub_rb.publish(control_gazebo_demo1);
        control_gazebo_demo1.data+=0.1;
        r.sleep();

/*
        for(control_gazebo_demo.data=0.0;control_gazebo_demo.data<=1.5;control_gazebo_demo.data+=0.01)
        {
		pub.publish(control_gazebo_demo);
 
		ROS_INFO("发送的消息:%f",control_gazebo_demo);
		r.sleep();
        }
 
        for(control_gazebo_demo.data=1.5;control_gazebo_demo.data>=0.0;control_gazebo_demo.data-=0.01)
        {
		pub.publish(control_gazebo_demo);
 
		ROS_INFO("发送的消息:%f",control_gazebo_demo);
		r.sleep();
        }
       */
        
        ros::spinOnce();
    }
    return 0;
}
